//
// Created by LiuDongPeng on 2024/1/25.
//
#include <cstdio>
#include "UserMain.h"
#include "cmsis_os.h"

#include "board.h"
#include "motor.h"
#include "utils.h"

#include "foc.h"

using MotorStatus = Motor::MotorStatus;


Board *board;
Motor* motor;


/**
 * @brief FreeRTOS thread
 */
osThreadId_t gVBusTaskHandle;
osThreadId_t gLedTaskHandle;
osThreadId_t gBtnScanTaskHandle;
const osThreadAttr_t gBtnScanTask_attributes = {
        .name = "gBtnScanTask",
        .stack_size = 256 * 4,
        .priority = (osPriority_t) osPriorityLow
};

void StartBtnScanTask(void *argument);
void StartLedTask(void *argument);
void StartVBusTask(void *argument);


/**
 * @brief 调试所需变量
 */
#define CH_COUNT 12
struct Frame
{
    float fdata[CH_COUNT];
    unsigned char tail[4];
};

struct Frame vfoaFrame = {
        .tail = {0x00, 0x00, 0x80, 0x7f}
};

float gDWQ;
float gUq = 0.1f;
float gIdRef = 0.0f;
float gIqRef = 0.1f;
float gSpeedRef = 60.0f;
float gPosRef = 1.0f;
float gDetTheta = 0.005f;
#define UART3_BUF_MAX_SIZE   4096
uint8_t vofaCmdBuf[1024];


/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

static void bsp_btn_event_callback(void *btn, int event);


/* USER CODE END FunctionPrototypes */




/**
 * @brief
 * @return
 */
int UserMain()
{
    board = new Board();
    motor = new Motor();
    motor->link_encoder(board->encoder_);
    motor->setTim(&htim1);
    motor->disable();

    button_install_event_callback(&board->btn1_, ButtonEvent_SingleClick, bsp_btn_event_callback);
    button_install_event_callback(&board->btn2_, ButtonEvent_SingleClick, bsp_btn_event_callback);
    button_install_event_callback(&board->btn3_, ButtonEvent_SingleClick, bsp_btn_event_callback);
    button_install_event_callback(&board->btn4_, ButtonEvent_SingleClick, bsp_btn_event_callback);

    /* vofa+ debug */
    HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);

    /* creation of gBtnScanTask */
    gBtnScanTaskHandle = osThreadNew(StartBtnScanTask, NULL, &gBtnScanTask_attributes);

    /* creation of gLedTask */
    gBtnScanTaskHandle = osThreadNew(StartLedTask, NULL, &gBtnScanTask_attributes);

    /* creation of gVBusTask */
    gVBusTaskHandle = osThreadNew(StartVBusTask, NULL, &gBtnScanTask_attributes);

    return 0;
}


/**
 * @brief Control motor work status
 * @param btn
 * @param event
 */
static void bsp_btn_event_callback(void *btn, int event)
{
    if (btn == &board->btn1_)
    {
        gUq += 0.5f;
        if (gUq > SQRT_3 * motor->VBus_)
            gUq = SQRT_3 * motor->VBus_;
    }
    else if (btn == &board->btn2_)
    {
        gUq -= 0.5f;
    }
    else if (btn == &board->btn3_)
    {
//        motor->align_encoder(1.0f, 0);
    }
    else if (btn == &board->btn4_)
    {
        if (event == ButtonEvent_SingleClick)
        {
            motor->status_ = motor->status_ == MotorStatus::MOTOR_STATUS_RUN ? MotorStatus::MOTOR_STATUS_STOP :
                             MotorStatus::MOTOR_STATUS_RUN;

            if (motor->status_ == MotorStatus::MOTOR_STATUS_RUN)
            {
                motor->enable();
            }
            else
            {
                motor->disable();
            }
        }
    }
    else
    {
    }

    board->led3_->toggle();
}


/**
* @brief Function implementing the gBtnScanTask thread.
* @param argument: Not used
* @retval None
*/
void StartBtnScanTask(void *argument)
{
    for (;;)
    {
        button_ticks();
        HAL_Delay(5);

        osDelay(1);
    }
}

/**
* @brief Function implementing the gLedTaskHandle thread.
* @param argument: Not used
* @retval None
*/
void StartLedTask(void *argument)
{
    for (;;)
    {
        board->led1_->toggle();
//        board->led2_->toggle();
//        board->led3_->toggle();

        HAL_Delay(500);

        osDelay(1);
    }
}

/**
* @brief Function implementing the gVBusTaskHandle thread.
* @param argument: Not used
* @retval None
*/
void StartVBusTask(void *argument)
{
    motor->enable();

    for (;;)
    {
        HAL_ADC_Start(&hadc2);
        vfoaFrame.fdata[2] = (float)(HAL_ADC_GetValue(&hadc2) * (3.3f / 4096) * 15);
        HAL_Delay(10);

        osDelay(1);
    }
}



/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    /* Calc ADC offset */
    static bool isCalcAdcOffsetOvered = false;
    static uint32_t iaOffset = 0, ibOffset = 0, icOffset = 0;
    const int measCnt = 20;
    static int measCntCopy = measCnt;
    if (hadc->Instance == ADC1 && !isCalcAdcOffsetOvered)
    {
        iaOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        ibOffset += HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
//        icOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);

        if (--measCntCopy <= 0)
        {
            board->iaOffset_ = iaOffset / measCnt;
            board->ibOffset_ = ibOffset / measCnt;
//            bsp_board.icOffset = icOffset / measCnt;

            isCalcAdcOffsetOvered = true;
        }
    }

    /* Current closed loop */
    if (hadc->Instance == ADC1 && isCalcAdcOffsetOvered)
    {
//        HAL_GPIO_WritePin(Encoder_MOSI_GPIO_Port, Encoder_MOSI_Pin, GPIO_PIN_SET);

        /* Get phase current. */
        auto [ia, ib, ic] = board->get_phase_current();
        motor->set_ia_ib_ic(ia, ib, ic);

        /* Get motor electricity angle. */
        if (!true)
        {
            if (motor->has_encoder())
            {
                auto [valid, encoderRaw, angleRad] = motor->get_encoder()->update();
                motor->setAngleRad(angleRad);
                motor->getTheta();
                motor->calc_sin_cos_theta();
            }
        }
        else
        {
            motor->theta_ += 0.007f;
            if (motor->theta_ > M_TWOPI)
            {
                motor->theta_ -= M_TWOPI;
            }
            motor->calc_sin_cos_theta();
        }

        /* Pll get angle */
//        encoder_pll_update(motor1.encoder, motor1.sincosTheta.sinTheta, motor1.sincosTheta.cosTheta);

        /* Current closed loop */
        if (motor->get_status() == MotorStatus::MOTOR_STATUS_RUN)
        {
            motor->open_loop_test(0, gUq);
//            motor->current_closed_loop(gIdRef, gIqRef);
        }

        /* Calc speed */
        if (true)
        {
//            static int sLastEncoderRaw = 0;
//            static uint32_t sLastUs = 0;
//
//            uint32_t us = get_sys_micros();
//            int encoderRaw = GetCurrentAbsTotalValue((long) motor1.encoderRawData);
//            float detRad = (float)(encoderRaw - sLastEncoderRaw) / ENCODER_CPR / (float)M_TWOPI;
//            motor1.speedRpm = detRad / ((us - sLastUs) / 1e6) * M_TWOPI;
//            motor1.speedRpm *= 60.0f;
//            motor1.speedRpm = window_filter(motor1.speedRpm, windowFilterBuf, WIN_BUF_SIZE);
//            motor1.speedRpm = low_pass_filter_work(&motor1.speedLdFilter, motor1.speedRpm);
//
//            sLastEncoderRaw = encoderRaw;
//            sLastUs = us;


            static int sLastEncoderRaw = 0;

//            int encoderRaw = GetCurrentAbsTotalValue((long) motor1.encoderRawData);
//            float detRad = (float)(encoderRaw - sLastEncoderRaw) * 20e3f * 60.0f / ENCODER_CPR;
//            motor1.speedRpm = window_filter(detRad, windowFilterBuf, WIN_BUF_SIZE);

//            sLastEncoderRaw = encoderRaw;
        }

        /* Speed closed loop */
        static uint32_t speedFbkCnt = 0;
        if (motor->get_status() == Motor::MotorStatus::MOTOR_STATUS_RUN && ++speedFbkCnt >= 5 && false)
        {
            speedFbkCnt = 0; // reset counter

        }

        /* Position closed loop */
        static uint32_t posFbkCnt = 0;
        if (motor->get_status() == MotorStatus::MOTOR_STATUS_RUN && ++posFbkCnt >= 20 && false)
        {
            posFbkCnt = 0; // reset counter

        }

        /* vofa debug */
        vfoaFrame.fdata[0] = ia;
        vfoaFrame.fdata[1] = ib;
        //vfoaFrame.fdata[2] = motor->theta_;
        vfoaFrame.fdata[3] = motor->angleRad_;
        if (false)
        {
            vfoaFrame.fdata[6] = (float) motor->ualpha_;
            vfoaFrame.fdata[7] = (float) motor->ubeta_;
        }
        if (false)
        {
            vfoaFrame.fdata[3] = (float) motor->ialpha_;
            vfoaFrame.fdata[4] = (float) motor->ibeta_;
        }
        if (false)
        {
            vfoaFrame.fdata[2] = (float) motor->id_;
            vfoaFrame.fdata[3] = (float) motor->iq_;
        }
        if (false)
        {
            vfoaFrame.fdata[10] = (float) motor->ud_;
            vfoaFrame.fdata[11] = (float) motor->uq_;
        }
        if (!false)
        {
            vfoaFrame.fdata[8] = (float) motor->ta_;
            vfoaFrame.fdata[9] = (float) motor->tb_;
            vfoaFrame.fdata[10] = (float) motor->tc_;
        }
        if (false)
        {
        }
        if (false)
        {
        }
        if (false)
        {
        }
        if (false)
        {
        }

        HAL_UART_Transmit_DMA(&huart3, (uint8_t *)(&vfoaFrame), sizeof(vfoaFrame));

//        HAL_GPIO_WritePin(Encoder_MOSI_GPIO_Port, Encoder_MOSI_Pin, GPIO_PIN_RESET);
    }
}


/**
 * @brief Parse vofa cmd
 * @param cmdBuf
 * @return
 */
static float vofa_cmd_parse(uint8_t *cmdBuf, char *arg)
{
    return atoff((const char*)(cmdBuf + strlen(arg)));
}

/**
 * @brief  Reception Event Callback (Rx event notification called after use of advanced reception service).
 * @param  huart UART handle
 * @param  Size  Number of data available in application reception buffer (indicates a position in
 *               reception buffer until which, data are available)
 * @retval None
 */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    if (huart->Instance == USART3)
    {
        if (strstr((const char*)(vofaCmdBuf), "uq"))
        {
            motor->uq_ = vofa_cmd_parse(vofaCmdBuf, "uq");
        }
        else if (strstr((const char*)(vofaCmdBuf), "idkp="))
        {
            float kp = vofa_cmd_parse(vofaCmdBuf, "idkp=");
//            motor1.idPid.kp = kp;
        }
        else if (strstr((const char*)(vofaCmdBuf), "idki="))
        {
            float ki = vofa_cmd_parse(vofaCmdBuf, "idkp=");
//            motor1.idPid.ki = ki;
        }
        else if (strstr((const char*)(vofaCmdBuf), "iqkp="))
        {
            float kp = vofa_cmd_parse(vofaCmdBuf, "iqkp=");
//            motor1.iqPid.kp = kp;
//            motor1.idPid.kp = kp;
        }
        else if (strstr((const char*)(vofaCmdBuf), "iqki="))
        {
            float ki = vofa_cmd_parse(vofaCmdBuf, "iqki=");
//            motor1.iqPid.ki = ki;
//            motor1.idPid.ki = ki;
        }
        else if (strstr((const char*)(vofaCmdBuf), "iqref="))
        {
            float iqref = vofa_cmd_parse(vofaCmdBuf, "iqref=");
            gIqRef = iqref;
        }
        else if (strstr((const char*)(vofaCmdBuf), "idref="))
        {
            float idref = vofa_cmd_parse(vofaCmdBuf, "idref=");
            gIdRef = idref;
        }
        else if (strstr((const char*)(vofaCmdBuf), "radOffset="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "radOffset=");
//            motor1.angleRadOffset = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "velref="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velref=");
            gSpeedRef = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "velkp="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velkp=");
//            motor1.speedPid.kp = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "velki="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velki=");
//            motor1.speedPid.ki = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "pos_ref="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_ref=");
            gPosRef = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "pos_kp="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_kp=");
//            motor1.positionPid.kp = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "pos_ki="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_ki=");
//            motor1.positionPid.ki = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "theta_offset="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "theta_offset=");
//            motor1.angleRadOffset = val;
        }
        else if (strstr((const char*)(vofaCmdBuf), "motor_en=") && false)
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "motor_en=");

            motor->set_status(motor->get_status() == MotorStatus::MOTOR_STATUS_RUN ?
                              MotorStatus::MOTOR_STATUS_STOP : MotorStatus::MOTOR_STATUS_RUN);

            if (motor->get_status() == MotorStatus::MOTOR_STATUS_RUN)
            {
                motor->enable();
            }
            else
            {
                motor->disable();
            }
        }
        else
        {
        }

        memset(vofaCmdBuf, 0, 1024);
        HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);
    }
}
